package simulator;
import java.awt.Graphics;
/**
 * This class models the behavior of a servo motor onto which 
 * a particular sensor has been mounted, rotating 
 * that sensor as it moves. Servo can provide a position value 
 * as a sensor reading, and can be set as a part of the robot
 * 
 * @author anand
 *
 */
public class Servo extends Sensor{
  RangeFinder[] sensors;
  double angle;
  public Servo(Environment e, RangeFinder[] r, double start){
	super(e);
    sensors= r;
    angle=start;
    if(sensors!=null)
    for(int i=0; i<sensors.length; i++){
        sensors[i].setMountAngle(sensors[i].getMountAngle() 
        				+ Math.toRadians(start));
        //Simulator.out.println(sensors[i].getMountAngle());
    }
  }
  
  public void rotate(double angle){
	//TODO(rmast) Fix rotation & position settings for servo
    turn(angle-this.angle);
    //this.angle=angle;
  }
  
  public void turn(double a){     //default couterclockwise
    for(int i=0; i<sensors.length; i++){
      sensors[i].setMountAngle(sensors[i].getMountAngle() 
      				+ Math.toRadians(a));
      //Simulator.out.println(sensors[i].getMountAngle());
    }
    System.out.println("xx" +a);
    System.out.println("rotated " +angle);
    this.angle += a;
  }
 public double[] read(int magicnum) {  
    double[] reading = new double[2];
    reading[0] = angle;
    reading[1] = Simulator.time;
    //System.out.println("Angle "+angle);
    setState(reading);
    return reading;
  }

  public void paint(Graphics g) { }
  
  public int getID() {
      return 5;
  }
  
  public String getName(){
    return "Servo"; //add id as well?
  }
  
}